#include "motiontracker.h"
#include "wii.h"
#include "player.h"

struct gestureBox {
    int maxTop, maxBottom, maxLeft, maxRight;
}m_gBox;

MotionTracker::MotionTracker(World *world, Player* player){

    //initialise class variables
    m_world = world;
    m_scene = world->getSceneManager();

    m_walkIndex = 1;
    m_normalise = true;

    m_lastSampleTime = 0;
    m_arrayIndex = 0;
    m_follower = 0;
    m_gestureArrayIndex = 0;
    gridDrawn = false;
    m_panelsDrawn = false;

    m_logger = Singleton::GetInstance();

    m_player = player;

    m_prev_1, m_prev_2, m_prev_3, m_prev_4 = 0;
    m_gBox.maxBottom, m_gBox.maxTop, m_gBox.maxLeft, m_gBox.maxRight = 0;

    for(int i = 0; i < TRAIL_LENGTH; i++){
        m_nodeArray[i] = m_scene->getRootSceneNode()->createChildSceneNode();
        m_nodeArray[i]->setPosition(0,0,0);
    }

    for(int i = 0; i < MAX_GESTURE; i++){
        m_gesturePath[i] = Ogre::Vector3(UNREACHABLE,UNREACHABLE,UNREACHABLE);
    }

    for(int i = 0; i < MAX_WALK; i++){
        m_walk[i] = Ogre::Vector2(UNREACHABLE, UNREACHABLE);
    }
}

/**
  * creates a marker to track wii motion at position passed as an Ogre::Vector
  *
  */

void MotionTracker::setTracker(Ogre::Vector3 wiiRelPosition){

    //set the adjustment vector
    Ogre::Vector3 adjustmentVector = Ogre::Vector3(-665,205,-120);//m_player->getPosition();

    //lock gesture to 2 dimensions
    wiiRelPosition.y = 0;

    //set circular array index and follower
    m_arrayIndex = Wii::mod(m_arrayIndex, TRAIL_LENGTH);
    m_follower = Wii::mod((m_arrayIndex + TRAIL_LENGTH - OFFSET), TRAIL_LENGTH);

    //create the scene node at arrayIndex
    m_nodeArray[m_arrayIndex] = m_scene->getRootSceneNode()->createChildSceneNode();

    if(FEEDBACK != 1){

    //create the sphere, attach it to node and set size
    Ogre::Entity* sphere = m_scene->createEntity("disk.mesh");
    m_nodeArray[m_arrayIndex]->attachObject(sphere);
    m_nodeArray[m_arrayIndex]->scale(0.1,0.1,0.1);

    //modify tracker size to simulate trail
    for(int i = 0; i < OFFSET; i++){
        int spherePos = Wii::mod(m_arrayIndex - i, TRAIL_LENGTH);
        m_nodeArray[spherePos]->setScale(TRACKER_SIZE-(i*(TRACKER_SIZE/OFFSET)),
                                         TRACKER_SIZE-(i*(TRACKER_SIZE/OFFSET)),
                                         TRACKER_SIZE-(i*(TRACKER_SIZE/OFFSET)));
    }
    }

    //initial normalizing of prev values
    if(m_normalise){
        normalise();
        m_normalise = false;
    }

    //apply low pass filter to input values
    wiiRelPosition = lowPassFilter(wiiRelPosition);

    //save last four vectors
    m_prev_4 = m_prev_3;
    m_prev_3 = m_prev_2;
    m_prev_2 = m_prev_1;
    m_prev_1 = wiiRelPosition;

    if(FEEDBACK != 1){

    //set position of sphere
    m_nodeArray[m_arrayIndex++]->setPosition(wiiRelPosition+adjustmentVector);

    }

    //save maximal gesture position in struct
    wiiRelPosition.x > m_gBox.maxLeft ? m_gBox.maxLeft = wiiRelPosition.x : m_gBox.maxLeft ;
    wiiRelPosition.x < m_gBox.maxRight ? m_gBox.maxRight = wiiRelPosition.x : m_gBox.maxRight;
    wiiRelPosition.z > m_gBox.maxTop ? m_gBox.maxTop = wiiRelPosition.z : m_gBox.maxTop;
    wiiRelPosition.z < m_gBox.maxBottom ? m_gBox.maxBottom = wiiRelPosition.z : m_gBox.maxBottom;

    //save sample of gesture
    m_time = m_world->getCurrentTime() - m_lastSampleTime;

    if(m_gestureArrayIndex < MAX_GESTURE && m_time > SAMPLE_TIME && m_world->m_gestureRunning){
        m_gesturePath[m_gestureArrayIndex++] = wiiRelPosition;
        m_lastSampleTime = m_world->getCurrentTime();
    }

    if(FEEDBACK != 1){
    //delete sphere at follower
    flush(m_follower);
    }
}

/**
  * analyse the gesture array in relation with predefined gesture patterns
  *
  */

void MotionTracker::analyseGesture(){

    std::cout << "\nGESTURE BOX EXTREMES:\n\n";

    std::cout << "Max. box left: " << m_gBox.maxLeft << "\n";
    std::cout << "Max. box right: " << m_gBox.maxRight << "\n";
    std::cout << "Max. box top: " << m_gBox.maxTop << "\n";
    std::cout << "Max. box bottom: " << m_gBox.maxBottom << "\n\n";

    // print out saved path
    for(int i = 0; i < MAX_GESTURE; i++){
        if(m_gesturePath[i].x > UNREACHABLE){
            //std::cout << "savedPath: "<< m_gesturePath[i] << "\n";
        }
    }

    //calculate grid values
    int boxWidth = m_gBox.maxLeft + abs(m_gBox.maxRight);
    int boxHeight = m_gBox.maxTop + abs(m_gBox.maxBottom);

    bool boxMinReached = false;

    //check box size
    if(boxWidth >= 100 && boxHeight >= 100){
        boxMinReached = true;
    }

    int vertBoundOne = m_gBox.maxRight + boxWidth / 3;
    int vertBoundTwo = m_gBox.maxRight + boxWidth / 3 * 2;
    int horizBoundOne = m_gBox.maxBottom + boxHeight / 3 * 2;
    int horizBoundTwo = m_gBox.maxBottom + boxHeight/3;

    std::cout << "GESTURE BOX PARAMETERS:\n\n";

    std::cout << "box width: " << boxWidth << "\n";
    std::cout << "box height: " << boxHeight << "\n\n";
    std::cout << "vertical bound one: " << vertBoundOne << "\n";
    std::cout << "vertical bound two: " << vertBoundTwo << "\n";
    std::cout << "horizontal bound one: " << horizBoundOne << "\n";
    std::cout << "horizontal bound two: " << horizBoundTwo << "\n\n";

    //determin grid walk
    Ogre::Vector2 atGridIndex;
    int atPathIndex = 0;

    while(m_gesturePath[atPathIndex].x > UNREACHABLE){

        // INCLUDE RECURSEIVE ALGORITHM HERE
        // give the two next points to function, if in box, add it to path (if box is not already in there)

         // first row

        if(inGridRectangle(m_gBox.maxTop, horizBoundOne, m_gBox.maxLeft, vertBoundTwo,
                           m_gesturePath[atPathIndex], m_gesturePath[atPathIndex+1])){

            //std::cout << "PATH INGRID BOX (1,1) \n";
            atGridIndex.x = 1;
            atGridIndex.y = 1;            
        }


        else if(inGridRectangle(m_gBox.maxTop, horizBoundOne, vertBoundTwo, vertBoundOne,
                           m_gesturePath[atPathIndex], m_gesturePath[atPathIndex+1])){

            //std::cout << "PATH INGRID BOX (1,2) \n";
            atGridIndex.x = 1;
            atGridIndex.y = 2;

        }
        else if(inGridRectangle(m_gBox.maxTop, horizBoundOne, vertBoundOne, m_gBox.maxRight,
                           m_gesturePath[atPathIndex], m_gesturePath[atPathIndex+1])){

            //std::cout << "PATH INGRID BOX (1,3) \n";
            atGridIndex.x = 1;
            atGridIndex.y = 3;

        }

        // second row
        else if(inGridRectangle(horizBoundOne, horizBoundTwo, m_gBox.maxLeft, vertBoundTwo,
                           m_gesturePath[atPathIndex], m_gesturePath[atPathIndex+1])){

            //std::cout << "PATH INGRID BOX (2,1) \n";
            atGridIndex.x = 2;
            atGridIndex.y = 1;

        }
        else if(inGridRectangle(horizBoundOne, horizBoundTwo, vertBoundTwo, vertBoundOne,
                           m_gesturePath[atPathIndex], m_gesturePath[atPathIndex+1])){

            //std::cout << "PATH INGRID BOX (2,2) \n";
            atGridIndex.x = 2;
            atGridIndex.y = 2;

        }
        else if(inGridRectangle(horizBoundOne, horizBoundTwo, vertBoundOne, m_gBox.maxRight,
                           m_gesturePath[atPathIndex], m_gesturePath[atPathIndex+1])){

            //std::cout << "PATH INGRID BOX (2,3) \n";
            atGridIndex.x = 2;
            atGridIndex.y = 3;

        }

        // first row
        else if(inGridRectangle(horizBoundTwo, m_gBox.maxBottom, m_gBox.maxLeft, vertBoundTwo,
                           m_gesturePath[atPathIndex], m_gesturePath[atPathIndex+1])){

            //std::cout << "PATH INGRID BOX (3,1) \n";
            atGridIndex.x = 3;
            atGridIndex.y = 1;

        }
        else if(inGridRectangle(horizBoundTwo, m_gBox.maxBottom, vertBoundTwo, vertBoundOne,
                           m_gesturePath[atPathIndex], m_gesturePath[atPathIndex+1])){

            //std::cout << "PATH INGRID BOX (3,2) \n";
            atGridIndex.x = 3;
            atGridIndex.y = 2;

        }
        else if(inGridRectangle(horizBoundTwo, m_gBox.maxBottom, vertBoundOne, m_gBox.maxRight,
                           m_gesturePath[atPathIndex], m_gesturePath[atPathIndex+1])){

            //std::cout << "PATH INGRID BOX (3,3) \n";
            atGridIndex.x = 3;
            atGridIndex.y = 3;

        }

        //if not yet in walk, put in walk and go to next pos
        if(m_walk[m_walkIndex-1] != atGridIndex && m_walkIndex <= MAX_WALK){
            m_walk[m_walkIndex] = atGridIndex;
            std::cout << "ADDING TO WALK: " << m_walk[m_walkIndex] << "\n";
            m_walkIndex++;
        }

        atPathIndex++;
    }
}

void MotionTracker::identifyGesture(){

    //define set of accepted gesture walks

    //SET 1

    Ogre::Vector2 spreadOne[5] = {{3,1},{3,2},{3,3},{2,3},{1,3}}; //circle
    Ogre::Vector2 shieldOne[5] = {{3,1},{2,1},{2,2},{2,3},{1,3}}; //Diagonal up
    Ogre::Vector2 flameOne[7] = {{1,1},{2,1},{3,1},{3,2},{2,2},{1,2},{1,3}}; //inverted S

    int spreadOneLength = 5;
    int shieldOneLength = 5;
    int flameOneLength = 7;

    //SET 2

    Ogre::Vector2 spreadTwo[7] = {{3,3},{3,2},{3,1},{2,1},{1,1},{1,2},{1,3}}; //inverted U
    Ogre::Vector2 shieldTwo[7] = {{2,2},{2,1},{3,1},{3,2},{3,3},{2,3},{1,3}}; //spiral
    Ogre::Vector2 flameTwo[9] = {{1,3},{1,2},{1,1},{2,1},{3,1},{3,2},{3,3},{2,3},{2,2}}; //id 2

    int spreadTwoLength = 7;
    int shieldTwoLength = 7;
    int flameTwoLength = 9;

    // old gestures

    Ogre::Vector2 circle[9] = {{2,3},{3,3},{3,2},{3,1},{2,1},{1,1},{1,2},{1,3},{2,3}}; //id 1
    //Ogre::Vector2 circle[8] = {{3,2},{3,1},{2,1},{1,1},{1,2},{1,3},{2,3},{3,3}}; //id 1
    Ogre::Vector2 diagonal[6] = {{2,1},{3,1},{3,2},{2,2},{2,3},{1,3}}; //id 2
    Ogre::Vector2 shield[7] = {{1,3},{1,2},{1,1},{2,1},{3,1},{3,2},{3,3}}; //id 3
    Ogre::Vector2 spike[8] = {{1,3},{2,3},{2,2},{2,1},{2,2},{2,3},{3,3}}; // id 4
    Ogre::Vector2 spiral[9] = {{2,2},{1,2},{1,3},{2,3},{3,3},{3,2},{3,1},{2,1},{1,1}}; //id 5
    Ogre::Vector2 snake[9] = {{3,3},{3,2},{3,1},{2,1},{2,2},{2,3},{1,3},{1,2},{1,1}}; //id 5

    // define gesture length

    int circleLength = 9;
    int diagonalLength = 6;
    int shieldLength = 7;
    int spikeLenghth = 8;
    int spiralLenghth = 9;
    int snakeLength = 9;

    // gesture detection init and helper variables

    if(GESTURE_SET == 1){

    bool foundGestureSpread = false;
    bool foundGestureSpreadWE = false;
    int errorSpread = 0;
    int errorsInGestureSpread = 0;
    bool lengthOkSpread = false;
    Ogre::Vector2 wrongGridSpread;
    int atErrorIndexSpread = -1;

    bool foundGestureShield = false;
    bool foundGestureShieldWE = false;
    int errorShield = 0;
    int errorsInGestureShield = 0;
    bool lengthOkShield = false;
    Ogre::Vector2 wrongGridShield;
    int atErrorIndexShield = -1;

    bool foundGestureSnake = false;
    bool foundGestureSnakeWE = false;
    int errorSnake = 0;
    int errorsInGestureSnake = 0;
    bool lengthOkSnake = false;
    Ogre::Vector2 wrongGridSnake;
    int atErrorIndexSnake = -1;

    int j = 0;

    int errorLimit = 1;

    int walkLength = 0;
    int walkLengthLimit = 4;

    //determin walk length of performed gesture (it is as long as the values are not equal to UNREACHABLE
    for(int i = 1; i < MAX_WALK; i++){
        if(m_walk[i].x == UNREACHABLE){
            break;
        }
        walkLength++;
    }

    //modify walk to detect error in the beginning and the end
    Ogre::Vector2 dummy = Ogre::Vector2(-5,-5);
    Ogre::Vector2 switchSave = m_walk[0];
    m_walk[0] = dummy;

    //----------------- DETECT GESTURE DIAGONAL ------------------------//
    // compare the performed walk to the gesture preset
    for(int i = 0; i < MAX_WALK; i++){
        for(j = 0; j < spreadOneLength; j++){
            if(m_walk[i+j] != spreadOne[j]){
                wrongGridSpread = spreadOne[j]; //save wrong step in grid where error occured
                errorSpread++;
                if(errorSpread > errorLimit){ //break early if more errors have been found than allowed in the limit
                    errorSpread = 0;
                    break; //augmenting i by one step
                }
            }
        }
        if(j >= spreadOneLength && errorSpread == 0){
            foundGestureSpread = true;
            break;
        }
        else if(j >= spreadOneLength && errorSpread != 0){
            foundGestureSpreadWE = true;
            errorsInGestureSpread = errorSpread;
            atErrorIndexSpread = i-spreadOneLength-1;
            break;
        }
    }

    // check walk length
    if(abs(walkLength - spreadOneLength) <= walkLengthLimit){
        lengthOkSpread = true;
    }

    //----------------- DETECT GESTURE CIRCLE ------------------------//
    // compare the performed walk to the gesture preset
    for(int i = 0; i < MAX_WALK; i++){
        for(j = 0; j < shieldOneLength; j++){
            if(m_walk[i+j] != shieldOne[j]){
                wrongGridShield = shieldOne[j]; //save wrong step in grid where error occured
                errorShield++;
                if(errorShield > errorLimit){ //break early if more errors have been found than allowed in the limit
                    errorShield = 0;
                    break; //augmenting i by one step
                }
            }
        }
        if(j >= shieldOneLength && errorShield == 0){
            foundGestureShield = true;
            break;
        }
        else if(j >= shieldOneLength && errorShield != 0){
            foundGestureShieldWE = true;
            errorsInGestureShield = errorShield;
            atErrorIndexShield = i-shieldOneLength-1;
            break;
        }
    }

    // check walk length
    if(abs(walkLength - shieldOneLength) <= walkLengthLimit){
        lengthOkShield = true;
    }

    //----------------- DETECT GESTURE FLAMER ------------------------//
    // compare the performed walk to the gesture preset
    for(int i = 0; i < MAX_WALK; i++){
        for(j = 0; j < snakeLength; j++){
            if(m_walk[i+j] != flameOne[j]){
                wrongGridSnake = flameOne[j]; //save wrong step in grid where error occured
                errorSnake++;
                if(errorSnake > errorLimit){ //break early if more errors have been found than allowed in the limit
                    errorSnake = 0;
                    break; //augmenting i by one step
                }
            }
        }
        if(j >= flameOneLength && errorSnake == 0){
            foundGestureSnake = true;
            break;
        }
        else if(j >= flameOneLength && errorSnake != 0){
            foundGestureSnakeWE = true;
            errorsInGestureSnake = errorSnake;
            atErrorIndexSnake = i-flameOneLength-1;
            break;
        }
    }

    // check walk length
    if(abs(walkLength - flameOneLength) <= walkLengthLimit){
        lengthOkSnake = true;
    }
    // react to gesture detection (draw grid, log data)

    if(foundGestureSpread && lengthOkSpread){
        m_logger->log("logging information ,gesture key, 11, diagonal gesture identified");
        Wii::wiiSetRumble(true);
        Wii::setGesture(CCR, true);

        drawGrid(spreadOne,spreadOneLength,Ogre::Vector2(-1,-1));

    }
    else if(foundGestureSpreadWE && lengthOkSpread){
        m_logger->log("logging information ,gesture key, 1, diagonal gesture identified containing errors");
        Wii::wiiSetRumble(true);
        drawGrid(spreadOne, spreadOneLength, wrongGridSpread);
        drawPanel(1, false);
        std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Wrong grid spread: " << wrongGridSpread<< "\n";
    }

    else if(foundGestureShield && lengthOkShield){
        m_logger->log("logging information ,gesture key, 11, circle gesture identified");
        Wii::wiiSetRumble(true);
        Wii::setGesture(CR, true);
        drawGrid(shieldOne,shieldOneLength,Ogre::Vector2(-1,-1));

    }
    else if(foundGestureShieldWE && lengthOkShield){
        m_logger->log("logging information ,gesture key, 1, circle gesture identified containing errors");
        Wii::wiiSetRumble(true);
        drawGrid(shieldOne, shieldOneLength, wrongGridShield);
        drawPanel(2, false);
        std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Wrong grid shield: " << wrongGridShield<< "\n";
    }
    else if(foundGestureSnake && lengthOkSnake){
        m_logger->log("logging information ,gesture key, 11, snake gesture identified");
        Wii::wiiSetRumble(true);
        Wii::setGesture(DU, true);
        drawGrid(flameOne,flameOneLength,Ogre::Vector2(-1,-1));

    }
    else if(foundGestureSnakeWE && lengthOkSnake){
        m_logger->log("logging information ,gesture key, 1, snake gesture identified containing errors");
        Wii::wiiSetRumble(true);
        drawGrid(flameOne,flameOneLength, wrongGridSnake);
        drawPanel(3, false);
        std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Wrong grid snake: " << wrongGridSnake<< "\n";
    }

    if(!foundGestureSnake && !foundGestureSnakeWE && !foundGestureShield && !foundGestureShieldWE &&
       !foundGestureSpread && !foundGestureSpreadWE){
        m_logger->log("logging information ,gesture key, 0, gesture attempt, but no gesture identified");
        drawPanel();
    }

    // do gesture cleanup
    gestureCleanup();
    }

    else if(GESTURE_SET == 2){

        bool foundGestureSpread = false;
        bool foundGestureSpreadWE = false;
        int errorSpread = 0;
        int errorsInGestureSpread = 0;
        bool lengthOkSpread = false;
        Ogre::Vector2 wrongGridSpread;
        int atErrorIndexSpread = -1;

        bool foundGestureShield = false;
        bool foundGestureShieldWE = false;
        int errorShield = 0;
        int errorsInGestureShield = 0;
        bool lengthOkShield = false;
        Ogre::Vector2 wrongGridShield;
        int atErrorIndexShield = -1;

        bool foundGestureSnake = false;
        bool foundGestureSnakeWE = false;
        int errorSnake = 0;
        int errorsInGestureSnake = 0;
        bool lengthOkSnake = false;
        Ogre::Vector2 wrongGridSnake;
        int atErrorIndexSnake = -1;

        int j = 0;

        int errorLimit = 1;

        int walkLength = 0;
        int walkLengthLimit = 4;

        //determin walk length of performed gesture (it is as long as the values are not equal to UNREACHABLE
        for(int i = 1; i < MAX_WALK; i++){
            if(m_walk[i].x == UNREACHABLE){
                break;
            }
            walkLength++;
        }

        //modify walk to detect error in the beginning and the end
        Ogre::Vector2 dummy = Ogre::Vector2(-5,-5);
        Ogre::Vector2 switchSave = m_walk[0];
        m_walk[0] = dummy;

        //----------------- DETECT GESTURE DIAGONAL ------------------------//
        // compare the performed walk to the gesture preset
        for(int i = 0; i < MAX_WALK; i++){
            for(j = 0; j < spreadTwoLength; j++){
                if(m_walk[i+j] != spreadTwo[j]){
                    wrongGridSpread = spreadTwo[j]; //save wrong step in grid where error occured
                    errorSpread++;
                    if(errorSpread > errorLimit){ //break early if more errors have been found than allowed in the limit
                        errorSpread = 0;
                        break; //augmenting i by one step
                    }
                }
            }
            if(j >= spreadTwoLength && errorSpread == 0){
                foundGestureSpread = true;
                break;
            }
            else if(j >= spreadTwoLength && errorSpread != 0){
                foundGestureSpreadWE = true;
                errorsInGestureSpread = errorSpread;
                atErrorIndexSpread = i-spreadTwoLength-1;
                break;
            }
        }

        // check walk length
        if(abs(walkLength - spreadTwoLength) <= walkLengthLimit){
            lengthOkSpread = true;
        }

        //----------------- DETECT GESTURE CIRCLE ------------------------//
        // compare the performed walk to the gesture preset
        for(int i = 0; i < MAX_WALK; i++){
            for(j = 0; j < shieldTwoLength; j++){
                if(m_walk[i+j] != shieldTwo[j]){
                    wrongGridShield = shieldTwo[j]; //save wrong step in grid where error occured
                    errorShield++;
                    if(errorShield > errorLimit){ //break early if more errors have been found than allowed in the limit
                        errorShield = 0;
                        break; //augmenting i by one step
                    }
                }
            }
            if(j >= shieldTwoLength && errorShield == 0){
                foundGestureShield = true;
                break;
            }
            else if(j >= shieldTwoLength && errorShield != 0){
                foundGestureShieldWE = true;
                errorsInGestureShield = errorShield;
                atErrorIndexShield = i-shieldTwoLength-1;
                break;
            }
        }

        // check walk length
        if(abs(walkLength - shieldTwoLength) <= walkLengthLimit){
            lengthOkShield = true;
        }

        //----------------- DETECT GESTURE FLAMER ------------------------//
        // compare the performed walk to the gesture preset
        for(int i = 0; i < MAX_WALK; i++){
            for(j = 0; j < flameTwoLength; j++){
                if(m_walk[i+j] != flameTwo[j]){
                    wrongGridSnake = flameTwo[j]; //save wrong step in grid where error occured
                    errorSnake++;
                    if(errorSnake > errorLimit){ //break early if more errors have been found than allowed in the limit
                        errorSnake = 0;
                        break; //augmenting i by one step
                    }
                }
            }
            if(j >= flameTwoLength && errorSnake == 0){
                foundGestureSnake = true;
                break;
            }
            else if(j >= flameTwoLength && errorSnake != 0){
                foundGestureSnakeWE = true;
                errorsInGestureSnake = errorSnake;
                atErrorIndexSnake = i-flameTwoLength-1;
                break;
            }
        }

        // check walk length
        if(abs(walkLength - flameTwoLength) <= walkLengthLimit){
            lengthOkSnake = true;
        }
        // react to gesture detection (draw grid, log data)

        if(foundGestureSpread && lengthOkSpread){
            m_logger->log("logging information ,gesture key, 11, diagonal gesture identified");
            Wii::wiiSetRumble(true);
            Wii::setGesture(CCR, true);

            drawGrid(spreadTwo,spreadTwoLength,Ogre::Vector2(-1,-1));

        }
        else if(foundGestureSpreadWE && lengthOkSpread){
            m_logger->log("logging information ,gesture key, 1, diagonal gesture identified containing errors");
            Wii::wiiSetRumble(true);
            drawGrid(spreadTwo, spreadTwoLength, wrongGridSpread);
            drawPanel(1,false);
            std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Wrong grid spread: " << wrongGridSpread<< "\n";
        }

        else if(foundGestureShield && lengthOkShield){
            m_logger->log("logging information ,gesture key, 11, circle gesture identified");
            Wii::wiiSetRumble(true);
            Wii::setGesture(CR, true);
            drawGrid(shieldTwo,shieldTwoLength,Ogre::Vector2(-1,-1));

        }
        else if(foundGestureShieldWE && lengthOkShield){
            m_logger->log("logging information ,gesture key, 1, circle gesture identified containing errors");
            Wii::wiiSetRumble(true);
            drawGrid(shieldTwo, shieldTwoLength, wrongGridShield);
            drawPanel(2,false);
            std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Wrong grid shield: " << wrongGridShield<< "\n";
        }
        else if(foundGestureSnake && lengthOkSnake){
            m_logger->log("logging information ,gesture key, 11, snake gesture identified");
            Wii::wiiSetRumble(true);
            Wii::setGesture(DU, true);
            drawGrid(flameTwo,flameTwoLength,Ogre::Vector2(-1,-1));

        }
        else if(foundGestureSnakeWE && lengthOkSnake){
            m_logger->log("logging information ,gesture key, 1, snake gesture identified containing errors");
            Wii::wiiSetRumble(true);
            drawGrid(flameTwo,flameTwoLength, wrongGridSnake);
            drawPanel(3,false);
            std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Wrong grid snake: " << wrongGridSnake<< "\n";
        }

        if(!foundGestureSnake && !foundGestureSnakeWE && !foundGestureShield && !foundGestureShieldWE &&
           !foundGestureSpread && !foundGestureSpreadWE){
            m_logger->log("logging information ,gesture key, 0, gesture attempt, but no gesture identified");
            //TODO drawPossibleGestures()
            drawPanel();
            // and if one error, draw corresponding gesture
        }

        // do gesture cleanup
        gestureCleanup();
    }
}

/**
  * computes the position where a sphere has to be drawn according to gesture
  *
  */

void MotionTracker::flush(int toDelete){

    m_nodeArray[toDelete]->getParent()->removeChild(m_nodeArray[toDelete]);
    delete m_nodeArray[toDelete];
}

void MotionTracker::deletePanels(){
    m_panelsDrawn = false;
    std::cout << "DELETING PANELS!!!\n";
    for(int i = 0; i < 3; i++){
        m_gestNode[i]->getParent()->removeChild(m_gestNode[i]);
        delete m_gestNode[i];
    }
}

void MotionTracker::deleteGrid(){

    // delete rectangle meshes
    for(int i = 0; i < 9; i++){
        m_recNode[i]->getParent()->removeChild(m_recNode[i]);
        delete m_recNode[i];
    }

    gridDrawn = false;
}

void MotionTracker::deleteMarkers(){}

/**
  * removes noise from accelerometer input values by using a recursive low pass filter
  *
  */

Ogre::Vector3 MotionTracker::lowPassFilter(Ogre::Vector3 smoothWiiVector){

    smoothWiiVector.x = WEIGHT_P1 * m_prev_1.x +
                         WEIGHT_P2 * m_prev_2.x +
                          WEIGHT_P3 * m_prev_3.x +
                          (1 - WEIGHT_P1 - WEIGHT_P2 - WEIGHT_P3) * smoothWiiVector.x;

    smoothWiiVector.y = WEIGHT_P1 * m_prev_1.y +
                         WEIGHT_P3 * m_prev_2.y +
                          WEIGHT_P3 * m_prev_3.y +
                          (1 - WEIGHT_P1 - WEIGHT_P2 - WEIGHT_P3) * smoothWiiVector.y;

    smoothWiiVector.z = WEIGHT_P1 * m_prev_1.z +
                         WEIGHT_P2 * m_prev_2.z +
                          WEIGHT_P3 * m_prev_3.z +
                          (1 - WEIGHT_P1 - WEIGHT_P2 - WEIGHT_P3) * smoothWiiVector.z;

    return smoothWiiVector;
}

void MotionTracker::drawPanel(){

    //draw all three panels (nothing identified)

    m_panelsDrawn = true;

    // create entities and nodes
    for(int i = 0; i < 3; i++){
        m_gesture[i] = m_scene->createEntity("red.mesh");
        m_gestNode[i] = m_scene->getRootSceneNode()->createChildSceneNode();
        m_gestNode[i]->attachObject(m_gesture[i]);
        m_gestNode[i]->scale(6,1,10);
    }

    if(GESTURE_SET == 1){
        m_gesture[0]->setMaterialName("sp1");
        m_gestNode[0]->setPosition(-350,200,1000);
        m_gesture[1]->setMaterialName("sh1");
        m_gestNode[1]->setPosition(-750,200,1000);
        m_gesture[2]->setMaterialName("fl1");
        m_gestNode[2]->setPosition(-1150,200,1000);
    }
    else if(GESTURE_SET == 2){
        m_gesture[0]->setMaterialName("sp2");
        m_gestNode[0]->setPosition(-350,200,1000);
        m_gesture[1]->setMaterialName("sh2");
        m_gestNode[1]->setPosition(-750,200,1000);
        m_gesture[2]->setMaterialName("fl2");
        m_gestNode[2]->setPosition(-1150,200,1000);
    }
}

void MotionTracker::drawPanel(int gesture,bool training){

    //draw single panel

    m_panelsDrawn = true;

    // create entities and nodes
    for(int i = 0; i < 3; i++){
        m_gesture[i] = m_scene->createEntity("red.mesh");
        m_gestNode[i] = m_scene->getRootSceneNode()->createChildSceneNode();
        m_gestNode[i]->attachObject(m_gesture[i]);
        m_gestNode[i]->scale(6,1,10);
    }

     m_gestNode[0]->setPosition(-1150,200,1000);
     m_gestNode[1]->setPosition(5000,5000,5000);
     m_gestNode[2]->setPosition(5000,5000,5000);

    if(GESTURE_SET == 1){

        // set materials
        if(gesture == 1){
            m_gesture[0]->setMaterialName("sp1");
        }
        else if(gesture == 2){
            m_gesture[0]->setMaterialName("sh1");
        }
        else if(gesture == 3){
           m_gesture[0]->setMaterialName("fl1");
       }
    }
    else if (GESTURE_SET == 2){

        // set materials
        if(gesture == 1){
            m_gesture[0]->setMaterialName("sp2");
        }
        else if(gesture == 2){
            m_gesture[0]->setMaterialName("sh2");
        }
        else if(gesture == 3){
           m_gesture[0]->setMaterialName("fl2");
        }
    }
}

/**
  * reset gesture states and prepare for next gesture
  *
  */

void MotionTracker::gestureCleanup(){

    m_normalise = true;

    m_gestureArrayIndex = 0;
    m_walkIndex = 1;

    m_prev_1 = 0; m_prev_2 = 0; m_prev_3 = 0; m_prev_4 = 0;

    m_gBox.maxBottom = 0; m_gBox.maxTop = 0; m_gBox.maxLeft = 0; m_gBox.maxRight = 0;

    for(int i = 0; i < MAX_GESTURE; i++){
        m_gesturePath[i] = Ogre::Vector3(UNREACHABLE,UNREACHABLE,UNREACHABLE);
    }

    for(int i = 0; i < MAX_WALK; i++){
        m_walk[i] = Ogre::Vector2(UNREACHABLE);
    }
}

/**
  * function to reset prev values at 0 (seems not to work in constructor)
  *
  */

void MotionTracker::normalise(){
    m_prev_1 = 0; m_prev_2 = 0; m_prev_3 = 0; m_prev_4 = 0;
}

bool MotionTracker::inGesture(Ogre::Vector2 gesture[],int gestureLength, Ogre::Vector2 toFind){
    for(int i = 0; i < gestureLength; i++){
        if (gesture[i] == toFind){
            return true;
        }
    }
    return false;
}

int MotionTracker::getPositionInGesture(Ogre::Vector2 gesture[],int gestureLength, Ogre::Vector2 toFind){
    for(int i = 0; i <= gestureLength; i++){
        if (gesture[i] == toFind){
            std::cout << "????????TO FIND: " << toFind << "GESTURE: " << i+1 << "\n";
            return i+1;
        }
    }
    return -1;
}

void MotionTracker::drawSquare(int squareNumber, int positionNumber,Ogre::Vector3 boxLocation,int color){

    //set position of node
    Ogre::Vector3 posAdjustment = Ogre::Vector3(-665,200,-120);
    m_recNode[squareNumber]->setPosition(boxLocation + posAdjustment);

    if(positionNumber == -1){
        positionNumber = 1;
    }

    switch(positionNumber){
    case 1:
        if(color == COLOR_RED){
              m_rectangle[squareNumber]->setMaterialName("r9");
        }
        else if(color == COLOR_GREEN){
             m_rectangle[squareNumber]->setMaterialName("g9");
        }
        else if(color == COLOR_GREY){
             m_rectangle[squareNumber]->setMaterialName("grey");
        }
        break;
    case 2:
        if(color == COLOR_RED){
              m_rectangle[squareNumber]->setMaterialName("r8");
        }
        else if(color == COLOR_GREEN){
             m_rectangle[squareNumber]->setMaterialName("g8");
        }
        else if(color == COLOR_GREY){
             m_rectangle[squareNumber]->setMaterialName("grey");
        }
        break;
    case 3:
        if(color == COLOR_RED){
              m_rectangle[squareNumber]->setMaterialName("r7");
        }
        else if(color == COLOR_GREEN){
             m_rectangle[squareNumber]->setMaterialName("g7");
        }
        else if(color == COLOR_GREY){
             m_rectangle[squareNumber]->setMaterialName("grey");
        }
        break;
    case 4:
        if(color == COLOR_RED){
              m_rectangle[squareNumber]->setMaterialName("r6");
        }
        else if(color == COLOR_GREEN){
             m_rectangle[squareNumber]->setMaterialName("g6");
        }
        else if(color == COLOR_GREY){
             m_rectangle[squareNumber]->setMaterialName("grey");
        }
        break;
    case 5:
        if(color == COLOR_RED){
              m_rectangle[squareNumber]->setMaterialName("r5");
        }
        else if(color == COLOR_GREEN){
             m_rectangle[squareNumber]->setMaterialName("g5");
        }
        else if(color == COLOR_GREY){
             m_rectangle[squareNumber]->setMaterialName("grey");
        }
        break;
    case 6:
        if(color == COLOR_RED){
              m_rectangle[squareNumber]->setMaterialName("r4");
        }
        else if(color == COLOR_GREEN){
             m_rectangle[squareNumber]->setMaterialName("g4");
        }
        else if(color == COLOR_GREY){
             m_rectangle[squareNumber]->setMaterialName("grey");
        }
        break;
    case 7:
        if(color == COLOR_RED){
              m_rectangle[squareNumber]->setMaterialName("r3");
        }
        else if(color == COLOR_GREEN){
             m_rectangle[squareNumber]->setMaterialName("g3");
        }
        else if(color == COLOR_GREY){
             m_rectangle[squareNumber]->setMaterialName("grey");
        }
        break;
    case 8:
        if(color == COLOR_RED){
              m_rectangle[squareNumber]->setMaterialName("r2");
        }
        else if(color == COLOR_GREEN){
             m_rectangle[squareNumber]->setMaterialName("g2");
        }
        else if(color == COLOR_GREY){
             m_rectangle[squareNumber]->setMaterialName("grey");
        }
        break;
    case 9:
        if(color == COLOR_RED){
              m_rectangle[squareNumber]->setMaterialName("r1");
        }
        else if(color == COLOR_GREEN){
             m_rectangle[squareNumber]->setMaterialName("g1");
        }
        else if(color == COLOR_GREY){
             m_rectangle[squareNumber]->setMaterialName("grey");
        }
        break;
    }
}


/**
  * function that draws the grid onto the screen
  *
  */

void MotionTracker::drawGrid(Ogre::Vector2 gesture[],int gestureLength, Ogre::Vector2 errorPos){

    if(FEEDBACK != 1 && FEEDBACK != 2){

    gridDrawn = true;

    //calculate grid values
    int boxWidth = m_gBox.maxLeft + abs(m_gBox.maxRight);
    int boxHeight = m_gBox.maxTop + abs(m_gBox.maxBottom);

    int vertBoundOne = m_gBox.maxRight + boxWidth / 3;
    int vertBoundTwo = m_gBox.maxRight + boxWidth / 3 * 2;
    int horizBoundOne = m_gBox.maxBottom + boxHeight / 3 * 2;
    int horizBoundTwo = m_gBox.maxBottom + boxHeight/3;

    int vertOne = (m_gBox.maxLeft + vertBoundTwo)/2;
    int vertTwo = (vertBoundTwo + vertBoundOne)/2;
    int vertThree = (vertBoundOne + m_gBox.maxRight)/2;

    int horizOne = (m_gBox.maxTop + horizBoundOne)/2;
    int horizTwo = (horizBoundOne + horizBoundTwo)/2;
    int horizThree = (horizBoundTwo + m_gBox.maxBottom)/2;

    float gridBoxWidth = boxWidth/55;
    float gridBoxHeight = boxHeight/55;

    Ogre::Vector3 adjustment(-665,200,-120);
    Ogre::Vector3 boxLocation(0,0,0);
    int spacingAdjustment = 15;
    int errorPosition = 0;

    // creating the rectangle meshes
    for(int i = 0; i < 9; i++){
        m_rectangle[i] = m_scene->createEntity("red.mesh");
        m_recNode[i] = m_scene->getRootSceneNode()->createChildSceneNode();
        m_recNode[i]->attachObject(m_rectangle[i]);
        m_recNode[i]->scale(gridBoxWidth/2.7,2,gridBoxHeight/2.7);
    }

    //checking grids

    // Position (1,1)
    boxLocation.x = vertOne + (boxWidth/spacingAdjustment);
    boxLocation.z = horizOne + (boxHeight/spacingAdjustment);

    //print error Position

    std::cout << "ERROR POSITION: " << errorPos << "\n";

    int position = getPositionInGesture(gesture,gestureLength,Ogre::Vector2(1,1));

    if(errorPos.x == 1 && errorPos.y == 1){
        errorPosition = getPositionInGesture(gesture,gestureLength, errorPos);
        drawSquare(0,errorPosition,boxLocation,COLOR_RED);
    }
    else if(inGesture(gesture,gestureLength,Ogre::Vector2(1,1))){
        drawSquare(0,position,boxLocation,COLOR_GREEN);
    }
    else{
        drawSquare(0,position,boxLocation,COLOR_GREY);
    }

    // Position (1,2)
    boxLocation.x = vertTwo;
    boxLocation.z = horizOne + (boxHeight/spacingAdjustment);

    position = getPositionInGesture(gesture,gestureLength,Ogre::Vector2(1,2));

    if(errorPos.x == 1 && errorPos.y == 2){
        errorPosition = getPositionInGesture(gesture,gestureLength, errorPos);
        drawSquare(1,errorPosition,boxLocation,COLOR_RED);
    }
    else if(inGesture(gesture,gestureLength,Ogre::Vector2(1,2))){
        drawSquare(1,position,boxLocation,COLOR_GREEN);
    }
    else{
        drawSquare(1,position,boxLocation,COLOR_GREY);
        std::cout << "SETTING FUCKING GREY!!!\n";
    }

    // Position (1,3)
    boxLocation.x = vertThree - (boxWidth/spacingAdjustment);;
    boxLocation.z = horizOne + (boxHeight/spacingAdjustment);;

    position = getPositionInGesture(gesture,gestureLength,Ogre::Vector2(1,3));

    if(errorPos.x == 1 && errorPos.y == 3){
        errorPosition = getPositionInGesture(gesture,gestureLength, errorPos);
        drawSquare(2,errorPosition,boxLocation,COLOR_RED);
    }
    else if(inGesture(gesture,gestureLength,Ogre::Vector2(1,3))){
        drawSquare(2,position,boxLocation,COLOR_GREEN);
    }
    else{
        drawSquare(2,position,boxLocation,COLOR_GREY);
    }

    // Position (2,1)
    boxLocation.x = vertOne + (boxWidth/spacingAdjustment);
    boxLocation.z = horizTwo;

    position = getPositionInGesture(gesture,gestureLength,Ogre::Vector2(2,1));

    if(errorPos.x == 2 && errorPos.y == 1){
        errorPosition = getPositionInGesture(gesture,gestureLength, errorPos);
        drawSquare(3,errorPosition,boxLocation,COLOR_RED);
    }
    else if(inGesture(gesture,gestureLength,Ogre::Vector2(2,1))){
        drawSquare(3,position,boxLocation,COLOR_GREEN);
    }
    else{
        drawSquare(3,position,boxLocation,COLOR_GREY);
    }

    // Position (2,2)
    boxLocation.x = vertTwo;
    boxLocation.z = horizTwo;;

    position = getPositionInGesture(gesture,gestureLength,Ogre::Vector2(2,2));

    if(errorPos.x == 2 && errorPos.y == 2){
        errorPosition = getPositionInGesture(gesture,gestureLength, errorPos);
        drawSquare(4,errorPosition,boxLocation,COLOR_RED);
    }
    else if(inGesture(gesture,gestureLength,Ogre::Vector2(2,2))){
        drawSquare(4,position,boxLocation,COLOR_GREEN);
    }
    else{
        drawSquare(4,position,boxLocation,COLOR_GREY);
    }

    // Position (2,3)
    boxLocation.x = vertThree - (boxWidth/spacingAdjustment);
    boxLocation.z = horizTwo;

    position = getPositionInGesture(gesture,gestureLength,Ogre::Vector2(2,3));

    if(errorPos.x == 2 && errorPos.y == 3){
        errorPosition = getPositionInGesture(gesture,gestureLength, errorPos);
        drawSquare(5,errorPosition,boxLocation,COLOR_RED);
    }
    else if(inGesture(gesture,gestureLength,Ogre::Vector2(2,3))){
        drawSquare(5,position,boxLocation,COLOR_GREEN);

    }
    else{
        drawSquare(5,position,boxLocation,COLOR_GREY);
    }

    // Position (3,1)
    boxLocation.x = vertOne + (boxWidth/spacingAdjustment);;
    boxLocation.z = horizThree - (boxHeight/spacingAdjustment);

    position = getPositionInGesture(gesture,gestureLength,Ogre::Vector2(3,1));

    if(errorPos.x == 3 && errorPos.y == 1){
        errorPosition = getPositionInGesture(gesture,gestureLength, errorPos);
        drawSquare(6,errorPosition,boxLocation,COLOR_RED);
    }
    else if(inGesture(gesture,gestureLength,Ogre::Vector2(3,1))){
        drawSquare(6,position,boxLocation,COLOR_GREEN);

    }
    else{
        drawSquare(6,position,boxLocation,COLOR_GREY);
    }

    // Position (3,2)
    boxLocation.x = vertTwo;
    boxLocation.z = horizThree - (boxHeight/spacingAdjustment);

    position = getPositionInGesture(gesture,gestureLength,Ogre::Vector2(3,2));

    if(errorPos.x == 3 && errorPos.y == 2){
        errorPosition = getPositionInGesture(gesture,gestureLength, errorPos);
        drawSquare(7,errorPosition,boxLocation,COLOR_RED);
    }
    else if(inGesture(gesture,gestureLength,Ogre::Vector2(3,2))){
        drawSquare(7,position,boxLocation,COLOR_GREEN);

    }
    else{
        drawSquare(7,position,boxLocation,COLOR_GREY);
    }

    // Position (3,3)
    boxLocation.x = vertThree - (boxWidth/spacingAdjustment);
    boxLocation.z = horizThree - (boxHeight/spacingAdjustment);

    position = getPositionInGesture(gesture,gestureLength,Ogre::Vector2(3,3));

    if(errorPos.x == 3 && errorPos.y == 3){
        errorPosition = getPositionInGesture(gesture,gestureLength, errorPos);
        drawSquare(8,errorPosition,boxLocation,COLOR_RED);
    }
    else if(inGesture(gesture,gestureLength,Ogre::Vector2(3,3))){
        drawSquare(8,position,boxLocation,COLOR_GREEN);

    }
    else{
        drawSquare(8,position,boxLocation,COLOR_GREY);
    }
    }
}

/**
  * recursive divide and conquor function determining if a line segment intersects with a given rectangle
  *
  */

bool MotionTracker::inGridRectangle(int boxTop, int boxBottom, int boxLeft, int boxRight, Ogre::Vector3 pointOne, Ogre::Vector3 pointTwo){

    //std::cout << "recursion step: ";

    //initialize flags
    int pointOneFlag[4] = {0,0,0,0};
    int pointTwoFlag[4] = {0,0,0,0};

    //return point flags (check when setting flags if top,bottom, left, right are OK?)
    //Flag 0 = above, 1 = below, 2 = left, 3 right

    if(pointOne.z > boxTop){
        pointOneFlag[0] = 1;
    }
    else if(pointOne.z < boxBottom ){
        pointOneFlag[1] = 1;
    }

    if(pointOne.x > boxLeft){
        pointOneFlag[2] = 1;
    }
    else if(pointOne.x < boxRight ){
        pointOneFlag[3] = 1;
    }

    if(pointTwo.z > boxTop){
        pointTwoFlag[0] = 1;
    }
    else if(pointTwo.z < boxBottom ){
        pointTwoFlag[1] = 1;
    }

    if(pointTwo.x > boxLeft){
        pointTwoFlag[2] = 1;
    }
    else if(pointTwo.x < boxRight ){
        pointTwoFlag[3] = 1;
    }

    //check trivial: if all flags are set to 0, the point must be inside the grid box
    if(pointOneFlag[0] != 1 && pointOneFlag[1] != 1 && pointOneFlag[2] != 1 && pointOneFlag[3] != 1 ||
        pointTwoFlag[0] != 1 && pointTwoFlag[1] != 1 && pointTwoFlag[2] != 1 && pointTwoFlag[3] != 1){
        //std::cout << "returning true\n";
            return true;
    }

    //check trivial: if points are on opposite sides of squares line must intersect square

    //check trivial: if both points are top, bottom, left right line can't intersect grid box
    if(pointOneFlag[0] != 0  && pointTwoFlag[0] != 0 && pointOneFlag[0] == pointTwoFlag[0]){
        //std::cout << "returning false\n";
        return false;
    }
    if(pointOneFlag[1] != 0 && pointTwoFlag[1] != 0 && pointOneFlag[1] == pointTwoFlag[1]){
        //std::cout << "returning false\n";
        return false;
    }
    if(pointOneFlag[2] != 0 && pointTwoFlag[2] != 0 && pointOneFlag[2] == pointTwoFlag[2]){
        //std::cout << "returning false\n";
        return false;
    }
    if(pointOneFlag[3] != 0 && pointTwoFlag[3] != 0 && pointOneFlag[3] == pointTwoFlag[3]){
        //std::cout << "returning false\n";
        return false;
    }

    //no triviality found >> split edge in two
    Ogre::Vector3 intermediate;
    intermediate.x = (pointOne.x + pointTwo.x) / 2;
    intermediate.z = (pointOne.z + pointTwo.z) / 2;

    //std::cout << "no trivial case, splitting edge: " << intermediate << "\n";

    return ((inGridRectangle(boxTop, boxBottom, boxLeft, boxRight, pointOne, intermediate)) ||
            (inGridRectangle(boxTop, boxBottom, boxLeft, boxRight, intermediate, pointTwo)));

}
